
// 桶型放射源高度：0-55cm（包含圆环部分）
// 桶顶圆半径：20cm
// 截取点云片段高度：10-50cm
// 本检测模型原理：由固定点fixed_point_a寻找弧线点云最近点b，并求ab连线延伸半径长度后的点c，即为中心点


#include "center_detection/barrel_detector.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <cmath>
#include <geometry_msgs/msg/point_stamped.hpp>

namespace center_detection{

    BarrelDetector::BarrelDetector(){}

    void BarrelDetector::setParameters(rclcpp::Node* node) {
        node->get_parameter("barrel.search_start_z", params_.search_start_z);
        node->get_parameter("barrel.search_end_z", params_.search_end_z);
        node->get_parameter("barrel.search_step", params_.search_step);
        node->get_parameter("barrel.min_points_in_slice", params_.min_points_in_slice);
        node->get_parameter("barrel.barrel_height", params_.barrel_height);
        node->get_parameter("barrel.barrel_radius", params_.barrel_radius);
        
        node->get_parameter("barrel.fixed_point_a_x", params_.fixed_point_a_x);
        node->get_parameter("barrel.fixed_point_a_y", params_.fixed_point_a_y);

    }

    bool BarrelDetector::detectCenter(PointCloud cloud,
                                    geometry_msgs::msg::PointStamped& center_point){
        
        if(cloud->empty()){
            return false;
        }

        // 1. 提取圆环部分(50-10cm部分)
        float detected_z = NAN;
        auto ring_cloud = extractRingCloud(cloud, detected_z);
        if(std::isnan(detected_z)){
            return false;
        }

        if(static_cast<int>(ring_cloud->size())< params_.min_points_in_slice){
            return false;
        } 

        // 2. 投影到XY平面
        auto xy_cloud = projectToXY(ring_cloud);

        // 3. 计算barrel中心点
        float center_x, center_y;
        if(!calculateCenter(xy_cloud, center_x, center_y))
        {
            return false;
        };


        // 4. 设置中心点
        center_point.point.x = center_x;
        center_point.point.y = center_y;
        center_point.point.z = detected_z;
        center_point.header.frame_id = "map";


        // 桶中心检测日志
        RCLCPP_INFO(rclcpp::get_logger("xxxxbarrel_detectorxxxx"), 
                "Barrel center detected: (%.3f, %.3f, %.3f)", 
                center_x, center_y, detected_z);


 /*        // 5. 发布点云中心
        JsonData_convert coord_json;
        coord_json.set_from_twist(center_point.point.x, center_point.point.y, center_point.point.z);
        
        std::string json_string = coord_json.get_json_string();

        if(!json_string.empty() && json_string != "null") {
            std_msgs::msg::String msg;
            msg.data = json_string;
            center_coord_pub_->publish(msg);
        }

        return true; */
    
    }

    BarrelDetector::PointCloud BarrelDetector::extractRingCloud(const PointCloud& cloud,
                                                                float& detected_z){

        auto ring_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        detected_z = NAN;

        for(float current_z = params_.search_start_z;
            current_z > params_.search_end_z;
            current_z -= params_.search_step){

            ring_cloud->clear();

            //提取当前高度切片
            for(const auto& point : *cloud){
                if(point.z >= current_z &&
                   point.z <= params_.search_start_z)
                {
                    ring_cloud->push_back(point);
                }
            }

            //检查点云密度
            if(static_cast<int>(ring_cloud->size()) > params_.min_points_in_slice){
                detected_z = params_.barrel_height;
                break;
            }

        }

        return ring_cloud;
    }
 

    pcl::PointCloud<pcl::PointXYZI>::Ptr BarrelDetector::projectToXY(const PointCloud& cloud) {
            
        auto xy_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        xy_cloud->reserve(cloud->size());

        for(const auto& point : cloud->points) {
            pcl::PointXYZI xy_point;
            xy_point.x = point.x;
            xy_point.y = point.y;
            xy_point.z = 0; //投影到z=0平面
            xy_point.intensity = point.intensity;
            xy_cloud->push_back(xy_point);
        }

        return xy_cloud;
    }

    bool BarrelDetector::calculateCenter(const PointCloud& cloud, float& center_x, float& center_y) { 

        if(cloud->size() < 3) {
            return false;
        }

        // 定义固定点a
        geometry_msgs::msg::Point a;
        a.x = params_.fixed_point_a_x;
        a.y = params_.fixed_point_a_y;

        // 查找圆环上距离点a最近的点b
        pcl::PointXYZI nearest_point_b;
        float min_distance_to_a = std::numeric_limits<float>::max();
        for(const auto& point : *cloud)
        {
            float distance_sq = std::pow(point.x - a.x, 2) + std::pow(point.y - a.y, 2);
            if(distance_sq < min_distance_to_a)
            {
                min_distance_to_a = distance_sq;
                nearest_point_b = point;
            }
        }

        // 如果没有找到最近的点
        if(min_distance_to_a == std::numeric_limits<float>::max()) {
             RCLCPP_WARN(rclcpp::get_logger("xxxxbarrel_detectorxxxx"), 
                        "No nearest point found on ring.");
             return false;
        }

        // 计算向量 ab
        float ab_x = nearest_point_b.x - a.x;
        float ab_y = nearest_point_b.y - a.y;

        // 归一化向量 ab
        float ab_length = std::sqrt(ab_x * ab_x + ab_y * ab_y);
        if(ab_length < 1e-6) { // 避免除以零或点a和b重合
            RCLCPP_WARN(rclcpp::get_logger("xxxxbarrel_detectorxxxx"), 
                        "Point a and nearest point b are too close or identical.");
            return false;
        }
        float unit_ab_x = ab_x / ab_length;
        float unit_ab_y = ab_y / ab_length; 

        // 计算中心点 c：从点 b 沿着向量 ab 的方向延伸一个半径长度
        // C = B + radius * unit_vector_AB
        center_x = nearest_point_b.x + params_.barrel_radius * unit_ab_x;
        center_y = nearest_point_b.y + params_.barrel_radius * unit_ab_y;

        return true;

    }

    

}